import math
import numpy as np

def xorFrombytesString(message):
    '''
        输入十六进制字符串，输出其异或值
        - in:'A5 84 04 70 00 00 00'
        - out: 55
    '''
    bit = 0
    num_list = [int(i,16) for i in message.split()]
    for num in num_list:
        bit^=num
    hexbit = hex(bit)
    if len(hexbit) == 3:
        result = '0'+hexbit[-1:]
    else:
        result = hexbit[-2:]
    return result

def calcPayloadSize(payload):
    '''
        计算负载长度
        - in: '00 00 00 00'
        - out: '04'
    '''
    payload_size = len(payload.split())
    if payload_size <16: 
        payload_size = '0'+hex(payload_size)[-1:]
    else:
        payload_size = hex(payload_size)[-2:]
    return payload_size

def signedBinToDec(binary,decilen):
    '''
        将有符号二进制小数转为十进制
        params:原始小数字符串binary，小数点后的位数decilen
        - in: '111011',3
        - out: -3.375
    '''
    if binary[:1] == '1':
        return 0-int(binary[1:],2)/math.pow(2,decilen)
    else:
        return int(binary,2)/math.pow(2,decilen)

def angleDiff(start_angle,next_angle):
    '''
        计算前后两个角度的差的绝对值
    '''
    if start_angle < next_angle:
        return next_angle - start_angle
    else:
        return 360 + next_angle -start_angle

def getAngle(dangle,start,next_start,index):
    '''
        根据rplidar协议，计算当前点的绝对角度
        - params:
            - start:当前整包的起始角度
            - next_start:下一个整包的起始角度
            - dangle:当前测量点解包出的角度补偿量
            - index:当前测量点在整包中的下标
        - return：
            - 保留到两位小数的绝对角度
    '''
    src_angle = start+(angleDiff(start,next_start)/32.0)*index-signedBinToDec(dangle,3)
    angle = src_angle if src_angle < 360 else src_angle -360
    return round(angle,2)

def calcStartAngle(src_data):
    '''
        根据rplidar协议，解析当前整包的起始角度
        - params：
            - src_data:整包
        - return:
            - start_angle:起始角度
    '''
    binary_with_s = bin(int(src_data[6:8]+src_data[4:6],16))
    start_binary = binary_with_s[3:] if len(binary_with_s) == 18 else binary_with_s[2:]
    start_angle = int(start_binary,2)/64.0
    return start_angle

def calcDistance(cabin_binary):
    '''
        根据rplidar协议，根据二进制cabin解析当前cabin中两个测量点的距离
        - params:
            - cabin_binary:二进制的cabin
        - return:
            - distance1:cabin中第一个测量点的距离
            - distance2:cabin中第二个测量点的距离
    '''
    distance1 = int(cabin_binary[8:16]+cabin_binary[0:6],2)/1000.0
    distance2 = int(cabin_binary[24:32]+cabin_binary[16:22],2)/1000.0
    return distance1,distance2

def calcAngle(cabin_binary,start,next_start,index):
    '''
        根据rplidar协议,根据二进制cabin解析当前cabin中两个测量点的角度
        - params:
            - cabin_binary:二进制的cabin
            - start:当前整包的起始角度
            - next:下一个整包的起始角度
            - index:当前测量点在整包中的下标
        - return:
            - angle1:cabin中第一个测量点的绝对角度
            - angle2:cabin中第二个测量点的绝对角度
    '''
    dangle1 = cabin_binary[6:8]+cabin_binary[36:40]
    dangle2 = cabin_binary[22:24]+cabin_binary[32:36]
    angle1 = getAngle(dangle1,start,next_start,index+1)
    angle2 = getAngle(dangle2,start,next_start,index+2)
    return angle1,angle2

def parseCabinList(cabin_list,start,next_start):
    '''
        根据rplidar协议，解析整包中的所有cabin
        - params:
            - cabin_list 由整包中全部16个cabin组成的列表
            - start:整包的起始角度
            - next_start:下一个整包的起始角度
        - return:
            - result_list:由当前整包解析的所有(角度,距离)元组构成的列表,角度小在前
    '''
    index = 0
    result = {}
    for cabin in cabin_list:
        cabin_binary = bin(int(cabin,16))[2:].zfill(40)
        dis1,dis2 = calcDistance(cabin_binary)
        ang1,ang2 = calcAngle(cabin_binary,start,next_start,index)
        result[ang1] = dis1
        result[ang2] = dis2
        index+=2
    return sorted(zip(result.keys(),result.values()))

def getHeader(last_result,this_result,angle_diff):
    '''
        判断一个(角度,距离)元组列表是否包含360度扫描中的起始（即0°）
        - params
            - this_result:当前的(角度,距离)元组列表
            - last_result:上一次的(角度,距离)元组列表
        - return
            - index
                - 如果包含起始点,给出起始点在(角度,距离)元组列表的下标
            - include_header
                - True:说明该列表包含起始点
                - False:说明该列表不包含起始点
    '''
    index = -1
    include_header = False
    last_head = last_result[0][0]
    this_head = this_result[0][0]
    last_tail = last_result[len(last_result)-1][0]
    if abs(this_head-last_tail) > 1.5*angle_diff and last_tail-last_head < 1.5*angle_diff:
        include_header = True
        for i in range(len(this_result)-1):
            if this_result[i+1][0] - this_result[i][0] > 1.5*angle_diff:
                index = i+1
                break
    return index,include_header

def transformAxis(src_data,half_side,max_range):
    '''
        将(角度,距离)元组列表映射到可视化界面的二维坐标(x,y)列表
        - params
            - src_data:整包
            - half_side:可视化界面边长像素值的一半
            - max_range:可视化界面半径对应的实际距离
        - return
            - 需要可视化的二维坐标(x,y)列表
    '''
    result = []
    for elem in src_data:
        if elem[1] != 0.0:
            x = int(half_side + half_side*(elem[1]/max_range)*np.cos(elem[0]*np.pi/180.0))
            y = int(half_side + half_side*(elem[1]/max_range)*np.sin(elem[0]*np.pi/180.0))
            result.append((x,y))
    return result

if __name__ == '__main__':
    pass